﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace UrScriptProgram
{
    public class UrTranspayload
    {
        public static String GetScript()
        {
            String program = "";
            program += "def transpayload():\n";
            program += "  socket_open(\"192.168.68.100\",25000,\"mysocket\")\n";
            program += "  global firstPoint=[-3.1415926535897932,-1.5707963267948966,-1.5707963267948966,-1.5707963267948966,1.5707963267948966,-3.1415926535897932]\n";
            program += "  movej(firstPoint, a=0.3962634015954636, v=0.0471975511965976)\n";
            ////#region 未安装工具时的初始化
            program += "  set_gravity([0.0, 0.0, 9.82])\n";
            program += "  set_tcp(p[0.0,0.0,0.0,0.0,0.0,0.0])\n";
            program += "  set_payload(0.0, [0.0, 0.0, 0.0])\n";
            //program += "  sleep(1)\n";
            program += "  set_tool_communication(False, 115200, 0, 1, 1.5, 3.5)\n";
            program += "  set_tool_output_mode(0)\n";
            program += "  set_tool_digital_output_mode(0, 1)\n";
            program += "  set_tool_digital_output_mode(1, 1)\n";
            program += "  set_tool_voltage(0)\n";
            program += "  step_count_8551c09a_8cb1_4ba3_88c6_17a3d03323e5 = 0.0\n";
            program += "  thread Step_Counter_Thread_9fb2c9ad_9c19_40d4_b48b_286d09e11613():\n";
            program += "    while (True):\n";
            program += "      step_count_8551c09a_8cb1_4ba3_88c6_17a3d03323e5 = step_count_8551c09a_8cb1_4ba3_88c6_17a3d03323e5 + 1.0\n";
            program += "      sync()\n";
            program += "    end\n";
            program += "  end\n";
            program += "  run Step_Counter_Thread_9fb2c9ad_9c19_40d4_b48b_286d09e11613()\n";
            program += "  set_safety_mode_transition_hardness(1)\n";
            program += "  set_standard_analog_input_domain(0, 1)\n";
            program += "  set_standard_analog_input_domain(1, 1)\n";
            program += "  set_tool_analog_input_domain(0, 1)\n";
            program += "  set_tool_analog_input_domain(1, 1)\n";
            program += "  set_analog_outputdomain(0, 0)\n";
            program += "  set_analog_outputdomain(1, 0)\n";
            program += "  set_input_actions_to_default()\n";

            program += "  global secondPoint = [-3.1415926535897932,-1.5707963267948966,-1.5707963267948966,-1,1.5707963267948966,-3.1415926535897932]\n";
            program += "  global thirdPoint = [-3.1415926535897932,-1.5707963267948966,-1.5707963267948966,0,1.5707963267948966,-3.1415926535897932]\n";
            program += "  global fourthPoint = [-3.1415926535897932,-1.5707963267948966,-1.5707963267948966,0.78,1.5707963267948966,-3.1415926535897932]\n";
            program += "  global testPoint1 = [-2.2689280275926285, -1.5707963267948966, -1.5707963267948966, 1.264164325955186E-15, 1.5707963267948966, 1.5707963267948966]\n";
            program += "  global testPoint2 = [-2.2689280275926285, -1.5707516235164123, -1.7453292519943364, -0.7853981633974456, 1.5708117485045427, -1.570810619984826]\n";
            ////#endregion

            program += " def get_tcp_force_tool():  \n";
            program += "  force_torque = get_tcp_force()\n";
            program += "  force_B = p[ force_torque[0], force_torque[1], force_torque[2], 0, 0, 0 ]\n";
            program += "  torque_B = p[ force_torque[3], force_torque[4], force_torque[5], 0, 0, 0 ]\n";
            program += "  tcp = get_actual_tcp_pose()\n";
            program += "  rotation_BT = p[ 0, 0, 0, tcp[3], tcp[4], tcp[5] ]\n";
            program += "  force_T = pose_trans( pose_inv(rotation_BT), force_B )\n";
            program += "  torque_T = pose_trans( pose_inv(rotation_BT), torque_B )\n";
            program += "  force_torque_T = p[ force_T[0], force_T[1], force_T[2], torque_T[0], torque_T[1], torque_T[2] ]\n";
            program += "  return force_torque_T\n"; //将基于基坐标系的力转换成基于工具坐标系的力
            program += "  end\n";

            program += " sleep(1.0)\n";
            program += " global ft1 = get_tcp_force_tool()\n";
            program += " global P1 = get_actual_tcp_pose()\n";
            program += " global RPY1 = rotvec2rpy([P1[3],P1[4],P1[5]])\n";
            program += " global Rz1 =  RPY1[0]\n";
            program += " global Ry1 =  RPY1[1]\n";
            program += " global Rx1 =  RPY1[2]\n";
            program += " global R1 = [[cos(Rx1),-sin(Rx1),0],[sin(Rx1),cos(Rx1),0],[0,0,1]] * [[cos(Ry1),0,sin(Ry1)],[0,1,0],[-sin(Ry1),0,cos(Ry1)]] * [[1,0,0],[0,cos(Rz1),-sin(Rz1)],[0,sin(Rz1),cos(Rz1)]]\n";



            program += " movej(secondPoint, a=1.3962634015954636, v=1.0471975511965976)\n";
            program += " sleep(1.0)\n";
            program += " global ft2 = get_tcp_force_tool()\n";
            program += " global P2 = get_actual_tcp_pose()\n";
            program += " global RPY2 = rotvec2rpy([P2[3],P2[4],P2[5]])\n";
            program += " global Rz2 =  RPY2[0]\n";
            program += " global Ry2 =  RPY2[1]\n";
            program += " global Rx2 =  RPY2[2]\n";
            program += " global R2 = [[cos(Rx2),-sin(Rx2),0],[sin(Rx2),cos(Rx2),0],[0,0,1]] * [[cos(Ry2),0,sin(Ry2)],[0,1,0],[-sin(Ry2),0,cos(Ry2)]] * [[1,0,0],[0,cos(Rz2),-sin(Rz2)],[0,sin(Rz2),cos(Rz2)]]\n";


            program += "  movej(thirdPoint, a=1.3962634015954636, v=1.0471975511965976)\n";
            program += "  sleep(1.0)\n";
            program += "  global ft3 = get_tcp_force_tool()\n";
            program += " global P3 = get_actual_tcp_pose()\n";
            program += " global RPY3 = rotvec2rpy([P3[3],P3[4],P3[5]])\n";
            program += " global Rz3 = RPY3[0]\n";
            program += " global Ry3 = RPY3[1]\n";
            program += " global Rx3 = RPY3[2]\n";
            program += " global R3 = [[cos(Rx3),-sin(Rx3),0],[sin(Rx3),cos(Rx3),0],[0,0,1]] * [[cos(Ry3),0,sin(Ry3)],[0,1,0],[-sin(Ry3),0,cos(Ry3)]] * [[1,0,0],[0,cos(Rz3),-sin(Rz3)],[0,sin(Rz3),cos(Rz3)]]\n";


            program += "  movej(fourthPoint, a=1.3962634015954636, v=1.0471975511965976)\n";
            program += "  sleep(1.0)\n";
            program += "  global ft4 = get_tcp_force_tool()\n";
            program += " global P4 = get_actual_tcp_pose()\n";
            program += " global RPY4 = rotvec2rpy([P4[3],P4[4],P4[5]])\n";
            program += " global Rz4 =  RPY4[0]\n";
            program += " global Ry4 =  RPY4[1]\n";
            program += " global Rx4 =  RPY4[2]\n";
            program += " global R4 = [[cos(Rx4),-sin(Rx4),0],[sin(Rx4),cos(Rx4),0],[0,0,1]] * [[cos(Ry4),0,sin(Ry4)],[0,1,0],[-sin(Ry4),0,cos(Ry4)]] * [[1,0,0],[0,cos(Rz4),-sin(Rz4)],[0,sin(Rz4),cos(Rz4)]]\n";


            program += " sleep(1.0)\n";
            program += " global  M = [ft1[3],ft1[4],ft1[5],ft2[3],ft2[4],ft2[5],ft3[3],ft3[4],ft3[5],ft4[3],ft4[4],ft4[5]]\n";
            program += " global  F = [[0,ft1[2],-ft1[1],1,0,0],[-ft1[2],0,ft1[0],0,1,0],[ft1[2],-ft1[0],0,0,0,1],[0,ft2[2],-ft2[1],1,0,0],[-ft2[2],0,ft2[0],0,1,0],[ft2[2],-ft2[0],0,0,0,1],[0,ft3[2],-ft3[1],1,0,0],[-ft3[2],0,ft1[0],0,1,0],[ft3[2],-ft3[0],0,0,0,1],[0,ft4[2],-ft4[1],1,0,0],[-ft4[2],0,ft4[0],0,1,0],[ft4[2],-ft4[0],0,0,0,1]]\n";
            program += " global  COG = inv(transpose(F) * F) * transpose(F) * transpose(M)\n";
            //program += " global  COG_T = transpose(COG)\n";
            program += " global  cx = COG[0,0]\n";
            program += " global  cy = COG[1,0]\n";
            program += " global  cz = COG[2,0]\n";


            //program += " global  cx = COG_T[0]\n";
            //program += " global  cy = COG_T[1]\n";
            //program += " global  cz = COG_T[2]\n";

            program += " global FF = [ft1[0],ft1[1],ft1[2],ft2[0],ft2[1],ft2[2],ft3[0],ft3[1],ft3[2],ft4[0],ft4[1],ft4[2]]\n";
            program += " global R100 = R1[0,0]\n";
            program += " global R101 = R1[0,1]\n";
            program += " global R102 = R1[0,2]\n";
            program += " global R110 = R1[1,0]\n";
            program += " global R111 = R1[1,1]\n";
            program += " global R112 = R1[1,2]\n";
            program += " global R120 = R1[2,0]\n";
            program += " global R121 = R1[2,1]\n";
            program += " global R122 = R1[2,2]\n";

            program += " global R200 = R2[0,0]\n";
            program += " global R201 = R2[0,1]\n";
            program += " global R202 = R2[0,2]\n";
            program += " global R210 = R2[1,0]\n";
            program += " global R211 = R2[1,1]\n";
            program += " global R212 = R2[1,2]\n";
            program += " global R220 = R2[2,0]\n";
            program += " global R221 = R2[2,1]\n";
            program += " global R222 = R2[2,2]\n";

            program += " global R300 = R3[0,0]\n";
            program += " global R301 = R3[0,1]\n";
            program += " global R302 = R3[0,2]\n";
            program += " global R310 = R3[1,0]\n";
            program += " global R311 = R3[1,1]\n";
            program += " global R312 = R3[1,2]\n";
            program += " global R320 = R3[2,0]\n";
            program += " global R321 = R3[2,1]\n";
            program += " global R322 = R3[2,2]\n";

            program += " global R400 = R4[0,0]\n";
            program += " global R401 = R4[0,1]\n";
            program += " global R402 = R4[0,2]\n";
            program += " global R410 = R4[1,0]\n";
            program += " global R411 = R4[1,1]\n";
            program += " global R412 = R4[1,2]\n";
            program += " global R420 = R4[2,0]\n";
            program += " global R421 = R4[2,1]\n";
            program += " global R422 = R4[2,2]\n";

            program += " global RR = [[R100,R101,R102,1,0,0],[R110,R111,R112,0,1,0],[R120,R121,R122,0,0,1],[R200,R201,R202,1,0,0],[R210,R211,R212,0,1,0],[R220,R221,R222,0,0,1],[R300,R301,R302,1,0,0],[R310,R311,R312,0,1,0],[R320,R321,R322,0,0,1],[R400,R401,R402,1,0,0],[R410,R411,R412,0,1,0],[R420,R421,R422,0,0,1]]\n";
            program += " global BB = inv(transpose(RR) * RR) * transpose(RR) * transpose(FF)\n";
            //program += " global BB_T = transpose(BB)\n";
            program += " global Fx0 = BB[3,0]\n";
            program += " global Fy0 = BB[4,0]\n";
            program += " global Fz0 = BB[5,0]\n";


            //program += " global Fx0 = BB_T[3]\n";
            //program += " global Fy0 = BB_T[4]\n";
            //program += " global Fz0 = BB_T[5]\n";
            program += " global mm = sqrt(BB[0,0]*BB[0,0]+BB[1,0]*BB[1,0]+BB[2,0]*BB[2,0])/9.82\n";
            program += " global m1 = sqrt((ft1[0]-BB[0,0])*(ft1[0]-BB[0,0])+(ft1[1]-BB[1,0])*(ft1[1]-BB[1,0])+(ft1[2]-BB[2,0])*(ft1[2]-BB[2,0]))/9.82\n";
            program += " global m2 = sqrt((ft2[0]-BB[0,0])*(ft2[0]-BB[0,0])+(ft2[1]-BB[1,0])*(ft2[1]-BB[1,0])+(ft2[2]-BB[2,0])*(ft2[2]-BB[2,0]))/9.82\n";
            program += " global m3 = sqrt((ft3[0]-BB[0,0])*(ft3[0]-BB[0,0])+(ft3[1]-BB[1,0])*(ft3[1]-BB[1,0])+(ft3[2]-BB[2,0])*(ft3[2]-BB[2,0]))/9.82\n";
            program += " global m4 = sqrt((ft4[0]-BB[0,0])*(ft4[0]-BB[0,0])+(ft4[1]-BB[1,0])*(ft4[1]-BB[1,0])+(ft4[2]-BB[2,0])*(ft4[2]-BB[2,0]))/9.82\n";
            program += " global  m=(m1+m2+m3+m4)/4 \n";

            program += " set_payload(m,[cx,cy,cz])\n";//计算有效载荷和重心坐标，完成设置
            program += "  socket_send_string(cx,\"mysocket\")\n";
            program += "  socket_send_string(\" \",\"mysocket\")\n";
            program += "  socket_send_string(cy,\"mysocket\")\n";
            program += "  socket_send_string(\" \",\"mysocket\")\n";
            program += "  socket_send_string(cz,\"mysocket\")\n";
            program += "  socket_send_string(\" \",\"mysocket\")\n";
            program += "  socket_send_string(m,\"mysocket\")\n";
            program += "  socket_send_string(\" \",\"mysocket\")\n";//将计算结果发送

            //program += "  movej(testPoint1, a=1.3962634015954636, v=1.0471975511965976)\n";
            //program += "  sleep(2.0)\n";
            //program += "  global f02= get_tcp_force ()\n";
            //program += "  movej(testPoint2, a=1.3962634015954636, v=1.0471975511965976)\n";
            //program += "  sleep(2.0)\n";
            //program += "  global f03= get_tcp_force ()\n";
            program += "  movej(firstPoint, a=1.3962634015954636, v=1.0471975511965976)\n";//完成有效载荷和重心坐标设置后，机器人运动到另外两个姿态，观察力传感器数据
            program += "  socket_close(\"mysocket\")\n";

            program += "end\n";

            return program;
        }

        //public static String GetScript()
        //{

        //}
    }
}
